/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Transformers;

import Filters.FrequencyFilter;
import Filters.MWA;
import GUI.Buffer;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.LinkedList;

/**
 *
 * @author Cliff Laschet
 */
public class QRSDet {

    private LinkedList<Double> qrsQueue = new LinkedList<Double>(); //Max 8 elements
    private LinkedList<Double> noiseQueue = new LinkedList<Double>(); //Max 8 elements
    private LinkedList<Integer> rQueue = new LinkedList<Integer>(); //Max 8 elements
    private double peakTreshold = 0.0;
    private double rTreshold = 0.0;
    private double minPeakTimeDifference = 200.0;
    private double rBackTrackTime = 360.0;
    private int minPeakIndexDifference = (int) (minPeakTimeDifference / Buffer.getTimePerSample());
    private ArrayList<Integer> detectedQRS = new ArrayList<Integer>();
    private int derivativeWidth = (int) (80.0 / Buffer.getTimePerSample());
    private static double TH = 0.475;

    /**
     * 
     * @param source
     * @param sampleTime
     * @return 
     */
    public double[] preProcess(double[] source) {
        //Apply band pass filter from 5 to 11 Hz.
        double[] filteredSource = FrequencyFilter.filterFrequency(source, 5, 11);

        //Calculate derivative function
        double[] filteredTemp = new double[filteredSource.length];
        for (int i = 0; i < filteredSource.length; i++) {
            if (i == 0) {
                double delta = filteredSource[i + 1] - filteredSource[i];
                filteredTemp[i] = Math.abs(delta); //Nog bekijken : hier delen door timestep of gewoon 1?
            } else if (i == source.length - 1) {
                double delta = filteredSource[i] - filteredSource[i - 1];
                filteredTemp[i] = Math.abs(delta);
            } else {
                double delta = filteredSource[i + 1] - filteredSource[i - 1];
                filteredTemp[i] = Math.abs(delta);
            }
        }
        filteredSource = filteredTemp;

        //Apply a window moving average over the derivative of roughly 80 ms.
        int windowSize = (int) (80.0 / Buffer.getTimePerSample() / 2);
        filteredTemp = MWA.normal(filteredSource, windowSize, windowSize);
        filteredSource = filteredTemp;

        return filteredSource;
    }

    /**
     * 
     */
    public void initBuffers(double[] originalSource, double[] processed) {
        //Find the maximal peak, this is most likely a QRS peak.
        double maximum = -1;
        for (int i = 0; i < processed.length; i++) {
            if (processed[i] > maximum) {
                maximum = processed[i];
            }
        }

        //Set noise buffer elements to 0, and the qrs buffer to the maximum found.
        for (int i = 0; i < 8; i++) {
            noiseQueue.add(0.0);
            qrsQueue.add(maximum);
            int rInitValue = (int) (1000.0 / Buffer.getTimePerSample());
            rQueue.add(rInitValue);
        }

        this.updateTreshold();
        this.updateRTreshold();
    }

    /**
     * 
     * @param originalSource
     * @param source
     * @return 
     */
    public ArrayList<Integer> qrsDetection(double[] originalSource, double[] source) {
        int lastPeakIndex = 0;
        int currentIndex = 0;
        while (currentIndex != source.length - 1) {
            //System.out.println(Buffer.getTimePerSample());
            //4. If peak hasnt been found for 1.5 * R samples, search back for previous complex we missed.
            if (currentIndex > ((int) 1.5 * rTreshold + lastPeakIndex)) {
                currentIndex = (int) (lastPeakIndex + (rBackTrackTime / Buffer.getTimePerSample()));
                boolean classified = false;
                while (!classified && currentIndex != source.length - 1) {
                    if (peak(currentIndex, source)) {
                        if (source[currentIndex] > (peakTreshold / 2)) {
                            qrsQueue.remove();
                            qrsQueue.add(source[currentIndex]);
                            detectedQRS.add(currentIndex);
                            updateTreshold();
                            rQueue.remove();
                            rQueue.add(currentIndex - lastPeakIndex);
                            updateRTreshold();
                            lastPeakIndex = currentIndex;
                            currentIndex += (int) (minPeakTimeDifference / Buffer.getTimePerSample());
                            classified = true;
                        }
                    }
                    currentIndex++;
                }
            } else {
                if (peak(currentIndex, source)) {
                    //1. First check if there are larger peaks than this one in 200ms interval on both sides.
                    // If this is the case, ignore all this and move on to this peak for analysis.
                    int sizeLeft = currentIndex;
                    int sizeRight = source.length - currentIndex - 1;

                    if (sizeLeft > minPeakIndexDifference) {
                        sizeLeft = minPeakIndexDifference;
                    }
                    if (sizeRight > minPeakIndexDifference) {
                        sizeRight = minPeakIndexDifference;
                    }

                    int currentMaxIndex = currentIndex;
                    //Left side
                    for (int i = currentIndex - sizeLeft; i < currentIndex; i++) {
                        if (source[i] > source[currentMaxIndex]) {
                            currentMaxIndex = i;
                        }
                    }
                    //Right side
                    for (int j = currentIndex + sizeRight; j > currentIndex; j--) {
                        if (source[j] > source[currentMaxIndex]) {
                            currentMaxIndex = j;
                        }
                    }

                    //Now only continue if this is the highest peak in the 400 ms range
                    if (currentMaxIndex == currentIndex) {
                        //2. Check if orignal source contained both positive and negative slopes.
                        //If not , it was a baseline shift and we can continue with the next index.
                        sizeLeft = currentIndex;
                        sizeRight = source.length - currentIndex - 1;

                        if (sizeLeft > derivativeWidth) {
                            sizeLeft = derivativeWidth;
                        }
                        if (sizeRight > derivativeWidth) {
                            sizeRight = derivativeWidth;
                        }

                        double leftDerivative = (originalSource[currentIndex]
                                - originalSource[currentIndex - sizeLeft]) / (currentIndex
                                - (currentIndex - sizeLeft));
                        double rightDerivative = (originalSource[currentIndex]
                                - originalSource[currentIndex + sizeRight]) / (currentIndex
                                - (currentIndex + sizeRight));

                        if ((leftDerivative > 0 && rightDerivative < 0)
                                || (leftDerivative < 0 && rightDerivative > 0)) {
                            //3. Now check if its a QRS complex or just noise.
                            if (source[currentIndex] > peakTreshold) {
                                //QRS complex
                                qrsQueue.remove();
                                qrsQueue.add(source[currentIndex]);
                                detectedQRS.add(currentIndex);
                                updateTreshold();
                                rQueue.remove();
                                rQueue.add(currentIndex - lastPeakIndex);
                                updateRTreshold();
                                lastPeakIndex = currentIndex;
                                currentIndex += (int) (minPeakTimeDifference / Buffer.getTimePerSample());
                            } else {
                                //Noise
                                noiseQueue.remove();
                                noiseQueue.add(source[currentIndex]);
                                //lastNoisePeak = currentIndex;
                                updateTreshold();
                                currentIndex++;
                            }
                        } else {
                            currentIndex++; //Was a baseline shift, carry on.
                        }
                    } else { // Else just continue at the maximum peak found.
                        if (currentIndex > currentMaxIndex) {
                            currentIndex++;
                        } else {
                            currentIndex = currentMaxIndex + 1;
                        }
                    }
                } else {
                    currentIndex++;
                }
            }
        }
        System.out.println("Detected " + detectedQRS.size() + " QRS waves.");
        for (int i = 0; i < detectedQRS.size(); i++) {
            System.out.println(detectedQRS.get(i));
        }
        return detectedQRS;
    }

    /**
     * 
     */
    private void updateTreshold() {
        Double[] qrsValues = qrsQueue.toArray(new Double[0]);
        Arrays.sort(qrsValues);
        double qmedian = qrsValues[(int) (qrsValues.length / 2)];

        Double[] noiseValues = noiseQueue.toArray(new Double[0]);
        Arrays.sort(noiseValues);
        double nmedian = noiseValues[(int) (noiseValues.length / 2)];

        double thrsh, dmed;
        double temp;
        dmed = qmedian - nmedian;
        temp = dmed;
        temp *= TH;
        dmed = temp;
        thrsh = nmedian + dmed;
        this.peakTreshold = thrsh;
    }

    /**
     * 
     */
    private void updateRTreshold() {
        Integer[] rValues = rQueue.toArray(new Integer[0]);
        Arrays.sort(rValues);
        rTreshold = rValues[(int) (rValues.length / 2)];
    }

    /**
     * 
     * @param currentIndex
     * @param source
     * @return 
     */
    private boolean peak(int currentIndex, double[] source) {
        int sizeLeft = (int) (minPeakTimeDifference / Buffer.getTimePerSample());
        int sizeRight = (int) (minPeakTimeDifference / Buffer.getTimePerSample());

        if (currentIndex <= sizeLeft) {
            return false;
        }
        if (currentIndex >= source.length - sizeRight) {
            return false;
        } else {
            for (int i = currentIndex - sizeLeft; i < currentIndex-1; i++) {
                if(source[i] > source[currentIndex]){
                    return false;
                }
            }

            for(int i = currentIndex + 1; i < currentIndex + sizeRight; i++){
                if(source[i] > source[currentIndex]){
                    return false;
                }
            }
            return true;
        }
    }

    public double getPeakTreshold(){
        return this.peakTreshold;
    }
}
